import time
from electron_low_level.electron_low_level import ElectronLowLevel

class ElectronPlayer:
    def __init__(self):
        self.robot = ElectronLowLevel()
        self.connected = False
        
    def connect(self):
        """连接机器人"""
        try:
            if self.robot.connect():
                self.connected = True
                print("机器人已连接!")
                return True
        except Exception as e:
            print(f"连接失败: {e}")
            self.connected = False
            return False
        
    def disconnect(self):
        """断开连接"""
        if self.robot.disconnect():
            self.connected = False
            print("机器人已断开连接!")
            return True
        return False
        
    def play_image(self, image):
        """显示图像"""
        if not self.connected:
            return False
            
        self.robot.set_image_src(image)
        return self.robot.sync()
        
    def set_pose(self, j1=0, j2=0, j3=0, j4=0, j5=0, j6=0, enable=True):
        """设置机器人姿态"""
        if not self.connected:
            return False
            
        self.robot.set_joint_angles(j1, j2, j3, j4, j5, j6, enable)
        return self.robot.sync()
        
    def get_pose(self):
        """获取当前姿态"""
        if not self.connected:
            return None
            
        return self.robot.get_joint_angles()

    def debug_info(self):
        """返回调试信息"""
        return {
            "connected": self.connected,
            "timestamp": self.robot.time_stamp,
            "joint_angles": self.get_pose()
        }

class ElectronBotPlayer:
    def __init__(self):
        self.robot = ElectronLowLevel()
        self.current_sequence = []
        self.is_playing = False
        
    def load_sequence(self, sequence_data):
        """加载动作序列"""
        self.current_sequence = sequence_data
        
    def play(self):
        """播放当前动作序列"""
        if not self.current_sequence:
            return
            
        self.is_playing = True
        for frame in self.current_sequence:
            if not self.is_playing:
                break
                
            # 设置关节角度
            self.robot.set_joint_angles(
                frame['angles'][0],
                frame['angles'][1],
                frame['angles'][2],
                frame['angles'][3],
                frame['angles'][4],
                frame['angles'][5],
                frame.get('enable', True)
            )
            
            # 设置显示图像
            if 'image' in frame:
                self.robot.set_image_src(frame['image'])
                
            self.robot.sync()
            time.sleep(frame.get('duration', 0.033))  # 默认30fps
            
    def stop(self):
        """停止播放"""
        self.is_playing = False 